import serial
import struct
import time
ser=serial.Serial('/dev/ttyAMA1', 57600)

class robotdemand():
    def __init__(self):
        self.x=0
        self.y=0
        self.z=0
        self.theta1=0
        self.theta2=0
        self.theta3=0
        self.demandOrder=1
    def serWrite(self,demand):
        demandLength=len(demand)+2
        if self.demandOrder > 255:
            self.demandOrder=1
        self.demandOrder.to_bytes(1, byteorder='big', signed=False)
        byteHead = bytes.fromhex('FF FF 02')
        byteOrder=self.demandOrder.to_bytes(1,byteorder='big', signed=False)
        byteLength=demandLength.to_bytes(1,byteorder='big', signed=False)
        byteDemand=bytes(demand, encoding='utf-8')
        byteTail=bytes.fromhex('0D 0A')
        byteData=byteHead+byteOrder+byteLength+byteDemand+byteTail
        print(byteData)
        ser.write(byteData)
        # while True:
        #     size = ser.inWaiting()  # 获得缓冲区字符
        #     if size != 0:
        #         res = ser.read(size)  # 读取内容并显示
        #         print(res)
        #         if res == b'ACK':
        #             ser.flushInput
        #             break
        #         elif res == b'NACK':
        #             ser.write(byteData)
        #             ser.flushInput
        #     time.sleep(0.2)

        return byteData
    def start(self):
        self.serWrite('mov:start')
        self.demandOrder += 1
    def stop(self):
        self.serWrite('mov:stop')
        self.demandOrder += 1
    def zero(self):
        self.serWrite('mov:zero')
        self.demandOrder += 1
    def movRotate(self,theta1,theta2,theta3,v1,v2,v3):
        str='mov:rotate[{:.2f}],[{:.2f}],[{:.2f}],[{:.2f}],[{:.2f}],[{:.2f}]'.format(theta1,theta2,theta3,v1,v2,v3)
        self.serWrite(str)
        self.demandOrder += 1
    def getValue(self):
        self.serWrite('getValue')
        self.demandOrder += 1
        # while True:
        #     size = ser.inWaiting()  # 获得缓冲区字符
        #     if size != 0:
        #         res = ser.readline  # 读取内容并显示
        #         print(res)
        res=b'\xff\xff\x02\x04\n[21][23][34]\r\n'
        data=res[5:-1]
        data=data.decode()
    #    data=data[5:-1]
        print(data)
        num1=[]
        num2=[]
        for i in range(len(data)):
            if [i] == '[':
                num1.append(i)
        print(num1)
        for i in range(len(data)):
            if [i] == ']':
                num2.append(i)
        print(num2)
        angle1=data[num1[0]+1:num2[0]]
        angle2 = data[num1[1] + 1:num2[1]]
        angle3 = data[num1[2] + 1:num2[2]]


    def objectClose(self):
        self.serWrite('objectClose')
        self.demandOrder += 1
    def objectOpen(self):
        str='objectopen'
        self.serWrite(str)
        self.demandOrder += 1

if __name__ == "__main__":
    robot=robotdemand()
    robot.movRotate(21.3,42.3,43.3,33.2,43.2,43.7)
    robot.start()
    robot.stop()
    robot.getValue()

